/***************************************************************************
 创建者: 华磊
 开始时间: 2019.4.3
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************
 ***************************************************************************/
#include "positionregulator.h"
#include <QDebug>
#include "filteringalgorithm.h"

PositionRegulator::PositionRegulator(int axisIdIn, double deltaTimeIn, RegulatorConfigInfo configInfoIn, E_REGULATOR_TYPE typeIn)
{
    axisId=axisIdIn;
    deltaTime=deltaTimeIn;
    configInfo=configInfoIn;
    status=E_REGULATOR_STATUS_STOPED;
    regulatorType=typeIn;

    pidController=new PIDControl;
    filter=new FilteringAlgorithm;
    PIDInit(pidController,configInfo.propotial, configInfo.integration, configInfo.differential,
                        deltaTime, configInfoIn.minOutput, configInfoIn.maxOutput,
                        AUTOMATIC, DIRECT);
    if(E_REGULATOR_TYPE_POSITION==regulatorType)
    {

    }
    else  if(E_REGULATOR_TYPE_VELOCITY==regulatorType)
    {
        PID_set_feedforward_method(pidController,1);
    }


    PIDSetpointSet(pidController,0.0);
}

int PositionRegulator::getPositionRegulatorDebugInfo(RegulatorDebugInfo &infoOut)
{
    infoOut.axisId=axisId;
    infoOut.configInfo=configInfo;
    infoOut.deltaTime=deltaTime;
    infoOut.feedbackPosition=feedbackPosition;
    infoOut.regulatorType=regulatorType;
    infoOut.status=status;
    infoOut.targetPosition=targetPosition;
    infoOut.velCommand=velCommand;
    return 1;
}

bool PositionRegulator::isValid()
{
    return configInfo.isValid;
}

int PositionRegulator::setServoStatus(bool isServoOn)
{
    if(true==isServoOn)
    {
        status=E_REGULATOR_STATUS_RUNNING;
    }
    else
    {
        resetAll();
    }
    return 1;
}

int PositionRegulator::loopRegulatorOnce(double targetPositionIn, double feedbackPositionIn, double &velOutut)
{
    switch(status)
    {
    case E_REGULATOR_STATUS_STOPED:
    {
        velOutut=0;
        velCommand=velOutut;
        return 0;
    }
    case E_REGULATOR_STATUS_ERROR:
    {
        velOutut=0;
        velCommand=velOutut;
        return 0;
    }
    case E_REGULATOR_STATUS_RUNNING:
    {
        targetPosition=targetPositionIn;
        feedbackPosition=feedbackPositionIn;
        PIDSetpointSet(pidController, targetPositionIn);
        PIDInputSet(pidController,feedbackPositionIn);//targetPositionIn-feedbackPositionIn
        PIDCompute(pidController);
        velOutut=PIDOutputGet(pidController);
        velOutut=filter->arithmeticMeanfilter(velOutut,5);
//        qDebug()<<"loopRegulatorOnce velOutut="<<velOutut<<" targetPos="<<targetPosition
//                  <<" feedbackPos="<<feedbackPositionIn;
        velCommand=velOutut;
        return 1;
    }
    default:
    {
        velOutut=0;
        velCommand=velOutut;
        return 0;
    }
    }
    return 0;
}

int PositionRegulator::loopRegulatorOnce( double feedbackPositionIn, double &velOutut)
{

    switch(status)
    {
    case E_REGULATOR_STATUS_STOPED:
    {
        loopRegulatorOnce(feedbackPositionIn,feedbackPositionIn,velOutut);//没有使能的时候也需要更新状态，防止前馈计算错误。
        velOutut=0;
        velCommand=velOutut;
        return 0;
    }
    case E_REGULATOR_STATUS_ERROR:
    {
        velOutut=0;
        velCommand=velOutut;
        return 0;
    }
    case E_REGULATOR_STATUS_RUNNING:
    {
        return loopRegulatorOnce(targetPosition,feedbackPositionIn,velOutut);
    }
    default:
    {
        velOutut=0;
        velCommand=velOutut;
        return 0;
    }
    }
    return 0;
}

int PositionRegulator::setPositionPid(double pid_p, double pid_i, double pid_d,
                                      double errorLimitForIntegration, double feedforward)
{
    qDebug()<<"PositionRegulator::setPositionPid"<<pid_p<<pid_i<<pid_d<<errorLimitForIntegration<<feedforward;
    PIDTuningsSet(pidController,pid_p,pid_i,pid_d);
    PIDTuningsErrorLimitForIntegrationSet(pidController,errorLimitForIntegration);
    PIDTuningsFeedForwardSet(pidController,feedforward);
    return 1;
}

int PositionRegulator::setPositionInput(double positionInput)
{
    return PID_set_position_input(pidController,positionInput);
}

int PositionRegulator::quickStop()
{

    resetAll();
    return 1;
}

int PositionRegulator::stop()
{
    return quickStop();

}

int PositionRegulator::cease()
{
    return quickStop();
}

int PositionRegulator::resetAll()
{
    filter->resetAll();
    status=E_REGULATOR_STATUS_STOPED;
    PIDReset(pidController);
    return 1;
}

int PositionRegulator::startRun()
{
    status=E_REGULATOR_STATUS_RUNNING;
    return 1;
}

bool PositionRegulator::isInitialOk()
{
    return true;

}
